#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>         
#include <yaml-cpp/yaml.h>
#include <vector>
#include <string>
#include <utility>
#include <boost/filesystem.hpp>
#include <ros/package.h>   

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

class PCActivePublisher {
public:
    PCActivePublisher(ros::NodeHandle& nh) {
        ros::NodeHandle pnh("~");
        // 1. 从参数服务器获取参数 node_graph_path，并触发 graph_file_load 函数
        std::string nodeGraphPath;
        if (pnh.getParam("node_graph_path", nodeGraphPath)) {
            graph_file_load(nodeGraphPath);
        } else {
            ROS_ERROR("Failed to get parameter: node_graph_path");
        }

        // 2. 订阅话题 /current_map，数据类型为字符串，回调函数为 map_callback
        sub_ = nh.subscribe("/current_map", 1, &PCActivePublisher::map_callback, this);

        // 3. 发布话题 /map，数据类型为 PointCloud2
        pub_ = nh.advertise<sensor_msgs::PointCloud2>("/map", 5);

        // 4. 加载 nodes 中的所有点云文件，转化为 PointCloud2 类型，并保存到 vector<PointCloud2> clouds 中
        load_point_clouds();

        // 5. 发布 clouds 中的第一个元素
        // if (!clouds_.empty()) {
        //     pub_.publish(clouds_[0]);
        //     ROS_INFO("<pc_activate_publisher> Switch to node: %s", nodes_[0].first.c_str());
        // }
    }

private:
    void graph_file_load(const std::string& file_path) {
        try {
            // 1. 解析输入的文件路径（ros使用的类型，形如"$(find navigation_pkg)/config/graph.yaml"）
            std::string resolved_path = resolve_ros_path(file_path);

            // 2. 读取 yaml 文件，并保存里面的 block_name 和 pcd_file_path 的值到数组 vector<pair<string, string>> nodes
            YAML::Node config = YAML::LoadFile(resolved_path);
            for (const auto& node : config["nodes"]) {
                std::string block_name = node["block_name"].as<std::string>();
                std::string pcd_file_path = resolve_ros_path(node["pcd_file_path"].as<std::string>());
                nodes_.emplace_back(block_name, pcd_file_path);
            }
        } catch (const YAML::Exception& e) {
            ROS_ERROR("Failed to load YAML file: %s", e.what());
        }
    }

    std::string resolve_ros_path(const std::string& path) {
        // 简单实现解析 $(find package) 格式的路径
        size_t startPos = path.find("$(");
        if (startPos != std::string::npos) {
            size_t endPos = path.find(")", startPos);
            if (endPos != std::string::npos) {
                std::string command = path.substr(startPos + 2, endPos - startPos - 2);
                std::istringstream iss(command);
                std::string token;
                std::getline(iss, token, ' ');
                if (token == "find") {
                    std::getline(iss >> std::ws, token); // 获取包名
                    std::string package_path = ros::package::getPath(token);
                    return package_path + path.substr(endPos + 1);
                }
            }
        }
        return path;
    }

    void load_point_clouds() {
    for (const auto& node : nodes_) {
        sensor_msgs::PointCloud2 cloud;
        if (load_pcd_file(node.second, cloud)) {
            clouds_.push_back(cloud);
        } else {
            ROS_WARN("Failed to load PCD file: %s", node.second.c_str());
        }
    }
}

    bool load_pcd_file(const std::string& filename, sensor_msgs::PointCloud2& cloud) {
        // 创建 PCL 点云对象（PointXYZ 类型）
        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);

        // 加载 PCD 文件
        if (pcl::io::loadPCDFile(filename, *pcl_cloud) == -1) {
            ROS_ERROR("Failed to load PCD file: %s", filename.c_str());
            return false;
        }

        // 转换为 ROS 使用的 PointCloud2 格式
        pcl::toROSMsg(*pcl_cloud, cloud);

        // 设置 header 信息（用于坐标变换等）
        cloud.header.stamp = ros::Time::now();
        cloud.header.frame_id = "map";  // 假设点云数据属于 map 坐标系

        ROS_INFO("Successfully loaded PCD file: %s with %zu points", filename.c_str(), pcl_cloud->size());
        return true;
    }

    void map_callback(const std_msgs::String::ConstPtr& msg) {
        // 1. 根据得到的 node_name，从 nodes 中找到对应的下标
        std::string node_name = msg->data;
        for (size_t i = 0; i < nodes_.size(); ++i) {
            if (nodes_[i].first == node_name && i < clouds_.size()) {
                // 2. 根据下标，从 clouds 中找到对应的 PointCloud2 并发布
                pub_.publish(clouds_[i]);
                ROS_INFO("<pc_activate_publisher> Switch to node: %s", node_name.c_str());
                return;
            }
        }
        ROS_WARN("Node name not found: %s", node_name.c_str());
    }

    ros::Subscriber sub_;
    ros::Publisher pub_;
    std::vector<std::pair<std::string, std::string>> nodes_;
    std::vector<sensor_msgs::PointCloud2> clouds_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pc_active_publisher");
    ros::NodeHandle nh;
    PCActivePublisher publisher(nh);
    ros::spin();
    return 0;
}

/*
写一个 roscpp 节点 pc_active_publisher 实现以下功能：

初始化函数：
1. 从参数服务器获取参数 node_graph_path ， 并触发graph_file_load 函数
2. 订阅话题 /current_map ，数据类型为字符串，回调函数为  map_callback
3. 发布话题 /map ， 数据类型为 PointCloud2
4. 加载 nodes 中的所有点云文件，转化为 PointCloud2 类型，并保存到 vector<PointCloud2> clouds 中
5. 发布 clouds 中的 第一个元素

graph_file_load 函数：
1. 解析输入的文件路径（ros使用的类型，形如"$(find navigation_pkg)/config/graph.yaml"）
2. 读取 yaml 文件，并保存里面的 block_name 和 pcd_file_path 的值到数组  vector<pair<string, string>> nodes
3. 解析nodes 中的文件名（方式同上），并覆盖 nodes 中的对应值

map_callback 函数：
1. 根据得到的 node_name ，从 nodes 中找到对应的 下标
2. 根据下标，从 clouds 中找到对应的 PointCloud2并发布

注： yaml文件示例


*/ 
